#ifndef COORDINATEMANAGER_H
#define COORDINATEMANAGER_H

#include "MacroDefine/GeneralDefine.h"
#include "coordinatesolver.h"
#include "motionmessage.h"
#include "trajectoryudefine.h"
#include "utcalculate.h"



class CoordinateManager
{
public:
    CoordinateManager(RobotCommonParameter * roboParameterIn, MotionMessage *motionMessageIn);
    int initCoordinate();
    bool getInitialStatus();
    int writeCoordinateFile();
    int readCoordinateFile();

    /**
     * @brief getToolCoordinateInfo
     * @return
     */
    QVector<ToolCoordinateInfo> getToolCoordinateList();
    /**
     * @brief getUserCoordinateList
     * @return
     */
    QVector<UserCoordinateInfo> getUserCoordinateList();

    int getCurrentToolForwardMatrix(Frame &frameReturn);
    int getCurrentToolInverseMatrix(Frame &frameReturn);
    int getCurrentUserForwardMatrix(Frame &frameReturn);
    int getCurrentUserInverseMatrix(Frame &frameReturn);
    int getCurrentToolIndex();
    int getCurrentUserIndex();
    int activateToolCoordinate(int index);
    int activateUserCoordinate(int index);
    int setToolCoordinate(ToolCoordinateInfo& toolCoordinate);
    int setUserCoordinate(UserCoordinateInfo& userCoordinate);

//    int setJointPositon(vector<double> positionIn);

    /**
     * @brief pointCoordinateToJoint, 把pointPro转为关节类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    PointPro pointCoordinateToJoint(const PointPro pointIn, vector<double> positionInitialList);
    /**
     * @brief pointCoordinateToCartesian, 把pointPro转为笛卡尔类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    PointPro pointCoordinateToCartesian(const PointPro pointIn);

    Frame pointProToFrame(const PointPro& pointPro);
    PointPro frameToPointPro_CART(const Frame& f);
    int userFrameToPointPro_JOINT(const Frame& frameIn, PointPro& pointOut, string configString, vector<double> positionInitialList);
    int isPointFutCorrect(PointPro checkPoint, vector<double> initialJointPosition);

private:
    int pointDeleteCoordinate(PointPro& decartPoint);
    int pointAddCoordinate(PointPro& decartPoint);

    int readUserCoordinateList(QString fileName);//从文件读取用户坐标系列表。
    int writeUserCoordinateList(QString fileName);//保存用户坐标系列表到文件。
    int readToolCoordinateList(QString fileName);//从文件读取工具坐标系列表。
    int writeToolCoordinateList(QString fileName);//保存工具坐标系列表到文件。
    friend QDataStream& operator <<(QDataStream &out, PointPro &pointPro);
    friend QDataStream& operator >>(QDataStream &in, PointPro &pointPro);
    friend QDataStream& operator <<(QDataStream &out, Frame &frame);
    friend QDataStream& operator >>(QDataStream &in, Frame &frame);
    friend QDataStream& operator <<(QDataStream &out, ToolCoordinateInfo &toolCoordinate);
    friend QDataStream& operator >>(QDataStream &in, ToolCoordinateInfo &toolCoordinate);
    friend QDataStream& operator <<(QDataStream &out, UserCoordinateInfo &userCoordinate);
    friend QDataStream& operator >>(QDataStream &in, UserCoordinateInfo &userCoordinate);

    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);

private:

    QVector<ToolCoordinateInfo> toolCoordinateList;

    QVector<UserCoordinateInfo> userCoordinateList;

    int currentToolIndex;
    int currentUserIndex;

    /**
     * @brief userCoordinateFile
     */
    QString userCoordinateFile;
    /**
     * @brief toolCoordinateFile
     */
    QString toolCoordinateFile;

    CoordinateSolver *coordinateSolver;
//    vector<double> currentJointPosition;
    RobotCommonParameter * roboParameter;
    int robotId;

    MotionMessage *motionMessage;
    UtCalculate *utCalculate;
    bool isInitialOk;

};

#endif // COORDINATEMANAGER_H
